#include "FOC.h"
#include <math.h> // 包含数学函数库
#include "wk_system.h"
#include <string.h> 
#include <stdlib.h>
#include "flux_weakening.h"
#include "calculation_speed.h"






VOLTAGE_DQ_DEF Voltage_DQ;
CURRENT_ALPHA_BETA_DEF  Voltage_Alpha_Beta;
float theta = 0.0;           // 转子角度

TRANSF_COS_SIN_DEF Transf_Cos_Sin;


int16_t counter;



volatile uint8_t is_upcount = 0; // 标志位，用于区分向上和向下计数

volatile uint8_t task_1ms = 0; // 标志位，用于区分向上和向下计数
volatile uint8_t task_10ms = 0; // 标志位，用于区分向上和向下计数

CURRENT_ABC_DEF I_abc;
Current_Offset I_offset;

uint8_t sector_f;

uint8_t State;
CURRENT_ALPHA_BETA_DEF I_Alpha_Beta;

 CURRENT_DQ_DEF I_DQ;


PIDController pid_current_Iq;
PIDController pid_current_Id;
PIDController pid_seepd;
volatile uint32_t cntr;

int32_t g_timx_encode_count;

ENCODE_TypeDef g_encode;

float speed_motor; //电机速度


float Iq_ref;


float pid_iq_p;
float pid_iq_i;
float IQ_ref_set;          //上位机下发数据



int16_t speed_set; //电机速度














//初始化，开启定时器及互补通道，配置PWM输出
void board_config(void)
{



    tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_4,TRUE);

    
     //开启定时器1的溢出中断
     tmr_interrupt_enable(TMR1, TMR_OVF_INT, TRUE);
     tmr_interrupt_enable(TMR2, TMR_OVF_INT, TRUE);
     tmr_interrupt_enable(TMR4, TMR_OVF_INT, TRUE);


     //开启抢占通道组转换结束中断使
    adc_interrupt_enable(ADC1, ADC_PCCE_INT, TRUE);
    Get_Current_Offset(100);

    
    //pid初始化
//    PID_Init(&pid_current_Iq, 0.001f, 0.001f, 0.0f, Umax, -Umax,10000000000.0f,-10000000000.0f);
//    PID_Init(&pid_current_Id, 0.001f, 0.001f, 0.0f, Umax, -Umax,10000000000.0f,-10000000000.0f);
//    PID_Init(&pid_seepd, 0.005f, 0.00001f, 0.00f, 8.0f, -8.0f, 10000000000.0f, -10000000000.0f);
    
//    PID_Init(&pid_current_Iq, 0.000f, 0.000f, 0.0f, Umax, -Umax,10000000000.0f,-10000000000.0f);
//    PID_Init(&pid_current_Id, pid_iq_p, 0.001f, 0.0f, Umax, -Umax,10000000000.0f,-10000000000.0f);
//    PID_Init(&pid_seepd, 0.005f, 0.00001f, 0.00f, 8.0f, -8.0f, 10000000000.0f, -10000000000.0f);
    
    //低速调好
    PID_Init(&pid_current_Iq, 0.001f, 0.001f, 0.0f, Umax, -Umax,10000000000.0f,-10000000000.0f);
    PID_Init(&pid_current_Id, 0.001f, 0.001f, 0.0f, Umax, -Umax,10000000000.0f,-10000000000.0f);
    //PID_Init(&pid_seepd, 0.1f, 0.0001f, 0.00f, 8.0f, -8.0f, 10000000000.0f, -10000000000.0f);
    PID_Init(&pid_seepd, 0.2f, 0.0001f, 0.00f, 8.0f, -8.0f, 10000000000.0f, -10000000000.0f);         //使用瞬时速度
    //PID_Init(&pid_seepd, 0.001f, 0.005f, 0.00f, 8.0f, -8.0f, 10000000000.0f, -10000000000.0f);
    
    
    //PID_Init(&pid_seepd, 0.15f, 0.0001f, 0.00f, 8.0f, -8.0f, 10000000000.0f, -10000000000.0f);        //使用均值滤波速度
    
     // PID_Init(&pid_seepd, 0.1f, 0.001f, 0.00f, 8.0f, -8.0f, 10000000000.0f, -10000000000.0f);      //低速
      
    
    
      //PID_Init(&pid_seepd, 0.1f, 0.005f, 0.00f, 8.0f, -8.0f, 10000000000.0f, -10000000000.0f); 
    
    
      PID_Init(&pid_seepd, 0.2f, 0.0001f, 0.00f, 8.0f, -8.0f, 10000000000.0f, -10000000000.0f); 
      
    //新代码
//    PID_Init(&pid_current_Iq, 0.1f, 0.01f, 0.0f, Umax, -Umax,10000000000.0f,-10000000000.0f);
//    PID_Init(&pid_current_Id, 0.1f, 0.01f, 0.0f, Umax, -Umax,10000000000.0f,-10000000000.0f);
//    PID_Init(&pid_seepd, 0.1f, 0.0001f, 0.00f, 8.0f, -8.0f, 10000000000.0f, -10000000000.0f);

//    PID_Init(&pid_current_Iq, 0.001f, 0.01f, 0.0f, Umax, -Umax,10000000000.0f,-10000000000.0f);
//    PID_Init(&pid_current_Id, 0.001f, 0.01f, 0.0f, Umax, -Umax,10000000000.0f,-10000000000.0f);
//    PID_Init(&pid_seepd, 0.005f, 0.0001f, 0.00f, 8.0f, -8.0f, 10000000000.0f, -10000000000.0f);
    
    //新代码测试
//    PID_Init(&pid_current_Iq, 0.1f, 0.01f, 0.0f, Umax, -Umax,10000000000.0f,-10000000000.0f);
//    PID_Init(&pid_current_Id, 0.1f, 0.01f, 0.0f, Umax, -Umax,10000000000.0f,-10000000000.0f);
//    PID_Init(&pid_seepd, 0.005f, 0.0001f, 0.00f, 8.0f, -8.0f, 10000000000.0f, -10000000000.0f);
    
    //能转
//    PID_Init(&pid_current_Iq, 0.001f, 0.001f, 0.0f, Umax, -Umax,10000000000.0f,-10000000000.0f);
//    PID_Init(&pid_current_Id, 0.001f, 0.001f, 0.0f, Umax, -Umax,10000000000.0f,-10000000000.0f);
//    PID_Init(&pid_seepd, 0.01f, 0.00001f, 0.00f,Imax, -Imax, 10000000000.0f, -10000000000.0f);

     // 测试
//    PID_Init(&pid_current_Iq, 0.001f, 0.001f, 0.0f, Umax, -Umax,10000000000.0f,-10000000000.0f);
//    PID_Init(&pid_current_Id, 0.001f, 0.001f, 0.0f, Umax, -Umax,10000000000.0f,-10000000000.0f);
//    PID_Init(&pid_seepd, 0.01f, 0.00001f, 0.00f,Imax, -Imax, 10000000000.0f, -10000000000.0f);

    weak_ctrl.V_bus=Umax;


    //速度初始化 
    init_motor_speed_calculator(&myMotorState, 0.0f); // 初始化电机速度计算器状态


    State=START;


}




//foc电流环
void foc_current_loop(void)
{


    //获取角度


   theta= get_encode_angle();


   ClarkeTransform(I_abc,&I_Alpha_Beta);
   ParkTransform(I_Alpha_Beta,theta,&I_DQ);
   
   
   weak_ctrl.Vd_ref=Voltage_DQ.Vd;
   weak_ctrl.Vq_ref=Voltage_DQ.Vq;
    
   //WeakFluxControl(&weak_ctrl);
    
    Voltage_DQ.Vq=PID_Compute(&pid_current_Iq,Iq_ref, I_DQ.Iq);
    Voltage_DQ.Vd=PID_Compute(&pid_current_Id, weak_ctrl.Id_ref, I_DQ.Id);
    
//    Voltage_DQ.Vq=PID_Compute(&pid_current_Iq,IQ_ref_set, I_DQ.Iq);
//    Voltage_DQ.Vd=PID_Compute(&pid_current_Id, 0.0f, I_DQ.Id);
    
//     Voltage_DQ.Vd=PID_Compute(&pid_current_Id, 0.0f, I_DQ.Id);
//     Voltage_DQ.Vq=PID_Compute(&pid_current_Iq,Iq_ref, I_DQ.Iq);
    
    
    

//     Voltage_DQ.Vd=0.0f;
//     Voltage_DQ.Vq=5.0f;
   
   

//     Voltage_DQ.Vd=0.0f;
//     Voltage_DQ.Vq=2.0f;


    
    
    
    
    
    
    InverseParkTransform(Voltage_DQ.Vd,Voltage_DQ.Vq,theta,&Voltage_Alpha_Beta.Ialpha,&Voltage_Alpha_Beta.Ibeta);
    
//    Voltage_Alpha_Beta.Ialpha=1.0f;
//    Voltage_Alpha_Beta.Ibeta=0.0f;
    
    SVPWM_Calculate_GPT_2(Voltage_Alpha_Beta.Ialpha,Voltage_Alpha_Beta.Ibeta);

    // 更新转子角度（假设通过编码器获取）
//    theta += 0.000625f; // 假设每次增加0.01弧度
//    if (theta > 2 * PI) {
//        theta -= 2 * PI;
//    }


    
    
    
    
    
    
    
    
    
    //获取编码器的值
   // counter=tmr_counter_value_get(TMR2);
    
    
    
    
    


}



void foc_speed_loop(void)
{
    //Iq_ref=PID_Compute(&pid_seepd, speed_set, (s16)myMotorState.filtered_motor_speed_rpm); // 速度环PID计算
    Iq_ref=PID_Compute(&pid_seepd, speed_set, speed_motor); // 速度环PID计算
   
    static uint16_t countter;
    countter++;
    if(countter==100)
    {
      
//      pid_seepd.Kp=pid_iq_p;
//      pid_seepd.Ki=pid_iq_i;
      
//      pid_current_Id.Kp=pid_iq_p;
//      pid_current_Id.Ki=pid_iq_i;
      
//      pid_current_Iq.Kp=pid_iq_p;
//      pid_current_Iq.Ki=pid_iq_i;
      
    //printf("%.0f,%.2f,%.2f,%.2f\n",speed_motor,Iq_ref,I_DQ.Iq,I_DQ.Id);
    //printf("%.0f,%.2f,%d,%.2f\n",speed_motor,Iq_ref,speed_set,I_DQ.Iq);
      //printf("%.0f\n",speed_motor);
      //printf("")
    //printf("%.2f,%.2f,%.2f\n",IQ_ref_set,I_DQ.Iq,I_DQ.Id);
     //printf("%.1f,%.1f\n",I_DQ.Iq,I_DQ.Id);
      
      //printf("%.0f,%.0f,%.0f\n",speed_motor,myMotorState.filtered_motor_speed_rpm,g_encode.speed);
      //printf("%.0f\n",myMotorState.filtered_motor_speed_rpm);
      //printf("%.0f,%.0f,%.0f\n",speed_motor,myMotorState.filtered_motor_speed_rpm,g_encode.speed);
    countter=0;
    }
    
    //printf("%.0f\n",speed_motor);
    
    //printf("%.1f,%.1f\n",I_DQ.Iq,I_DQ.Id);
}





// void SVPWM_Calculate_GPT_2(float V_alpha, float V_beta) {
//    static float Ta,Tb,Tc;
//     float Vd = V_alpha/Udc;
//     float Vq = V_beta/Udc;
//
//     float T1, T2, T0;
//    static float sector;
//     float Vref = sqrtf(Vd * Vd + Vq * Vq);
//     float angle = atan2f(V_beta, V_alpha);
//
//     if (angle < 0) {
//         angle += 2 * PI;
//     }
//
//     sector = floorf(angle / (PI / 3)) + 1;
//     sector_f=sector;
//     float t1 = Vref * sinf((PI / 3) - fmodf(angle, PI / 3)) / sinf(PI / 3);
//     float t2 = Vref * sinf(fmodf(angle, PI / 3)) / sinf(PI / 3);
//
//     T1 = t1 ; // Normalize to PWM period
//     T2 = t2 ; // Normalize to PWM period
//     T0 = (1 - T1 - T2) / 2;
//
//     switch ((int)sector) {
//         case 1:
//             Ta = T1 + T2 + T0;
//             Tb = T2 + T0;
//             Tc = T0;
//             break;
//         case 2:
//             Ta = T1 + T0;
//             Tb = T1 + T2 + T0;
//             Tc = T0;
//             break;
//         case 3:
//             Ta = T0;
//             Tb = T1 + T2 + T0;
//             Tc = T2 + T0;
//             break;
//         case 4:
//             Ta = T0;
//             Tb = T1 + T0;
//             Tc = T1 + T2 + T0;
//             break;
//         case 5:
//             Ta = T2 + T0;
//             Tb = T0;
//             Tc = T1 + T2 + T0;
//             break;
//         case 6:
//             Ta = T1 + T2 + T0;
//             Tb = T0;
//             Tc = T1 + T0;
//             break;
//     }
//    
//    
//     
//        
//      tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_1, (uint16_t)(Ta*PWM_PERIOD));
//      tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_2, (uint16_t)(Tb*PWM_PERIOD));
//      tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_3, (uint16_t)(Tc*PWM_PERIOD));
//    
//    
//    
//    
// }






// 硬件配置宏（根据实际硬件修改）


void SVPWM_Calculate_GPT_2(float Valpha, float Vbeta) {
    // 预计算常量
    //const float sqrt3 = 1.73205080757f;  // √3
    const float sqrt3_2 = 0.86602540378f; // √3/2

    // 1. 扇区判断
   static int sector = 0;
   sector=0;
    const float Vref1 = Vbeta;
    const float Vref2 = (sqrt3*Valpha - Vbeta)/2.0f;
    const float Vref3 = (-sqrt3*Valpha - Vbeta)/2.0f;

    if(Vref1 > 0.0f) sector += 1;
    if(Vref2 > 0.0f) sector += 2;
    if(Vref3 > 0.0f) sector += 4;
        sector_f=sector;
    // 2. 基本矢量作用时间计算
    float X = sqrt3 * Vbeta / UDC;
    float Y = (1.5f*Valpha + sqrt3_2*Vbeta)/UDC;
    float Z = (-1.5f*Valpha + sqrt3_2*Vbeta)/UDC;
    
   static float T1 = 0, T2 = 0;
    switch(sector) {
        case 1:  T1 = Z;   T2 = Y;   break;
        case 2:  T1 = Y;   T2 = -X;  break;
        case 3:  T1 = -Z;  T2 = X;   break;
        case 4:  T1 = -X;  T2 = Z;   break;
        case 5:  T1 = X;   T2 = -Y;  break;
        case 6:  T1 = -Y;  T2 = -Z;  break;
        default: // 错误处理
            T1 = T2 = 0.5f;
            break;
    }

    // 3. 时间限幅处理
    // float sum_T = T1 + T2;
    // if(sum_T > 1.0f) {
    //     T1 /= sum_T;
    //     T2 /= sum_T;
    //     sum_T = 1.0f;
    // }

   //过调制
   Modulate_Vec_Tim(&T1,&T2);
   float sum_T = T1 + T2;



    // 4. 生成对称PWM波形
    const float ta = (1.0f - sum_T)/2.0f;
    const float tb = ta + T2;
    const float tc = tb + T1;

    // 5. 计算各通道占空比（带死区补偿）
   static float ch1, ch2, ch3;
    // switch(sector) {
    //     case 1:  ch1 = tb; ch2 = ta; ch3 = tc; break;
    //     case 2:  ch1 = ta; ch2 = tc; ch3 = tb; break;
    //     case 3:  ch1 = ta; ch2 = tb; ch3 = tc; break;
    //     case 4:  ch1 = tc; ch2 = tb; ch3 = ta; break;
    //     case 5:  ch1 = tc; ch2 = ta; ch3 = tb; break;
    //     case 6:  ch1 = tb; ch2 = tc; ch3 = ta; break;
    //     default: ch1 = ch2 = ch3 = 0.5f; break; // 错误状态
    // }
    switch(sector) {
        case 1:  ch1 = tb; ch2 = tc; ch3 = ta; break;
        case 2:  ch1 = tc; ch2 = ta; ch3 = tb; break;
        case 3:  ch1 = tc; ch2 = tb; ch3 = ta; break;
        case 4:  ch1 = ta; ch2 = tb; ch3 = tc; break;
        case 5:  ch1 = ta; ch2 = tc; ch3 = tb; break;
        case 6:  ch1 = tb; ch2 = ta; ch3 = tc; break;
        default: ch1 = ch2 = ch3 = 0.5f; break; // 错误状态
    }


    // 6. 限幅保护并设置PWM
    ch1 = fmaxf(fminf(ch1, 1.0f), 0.0f);
    ch2 = fmaxf(fminf(ch2, 1.0f), 0.0f);
    ch3 = fmaxf(fminf(ch3, 1.0f), 0.0f);

    // 应用死区时间补偿

    static uint16_t duty1,duty2,duty3;
    
     duty1= (uint16_t)(ch1*PWM_PERIOD);
     duty2 = (uint16_t)(ch2*PWM_PERIOD);
     duty3 = (uint16_t)(ch3*PWM_PERIOD);

//    duty1 = (duty1 < DEAD_TIME) ? 0 : (duty1 - DEAD_TIME);
//    duty2 = (duty2 < DEAD_TIME) ? 0 : (duty2 - DEAD_TIME);
//    duty3 = (duty3 < DEAD_TIME) ? 0 : (duty3 - DEAD_TIME);

    // 写入PWM寄存器
    tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_1, duty1);
    tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_2, duty2);
    tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_3, duty3);
}















// 采用对称SVPWM算法 + 快速扇区判断（无需角度计算）
//void SVPWM_Calculate_GPT_2(float V_alpha, float V_beta) {
//    float Udc1 = 36.0f; // 根据实际电压修改
//   static float Ta, Tb, Tc;
//    
//    // 归一化到[-Udc/2, Udc/2]
////    float Vd = V_alpha / Udc1;
////    float Vq = V_beta / Udc1;
//     float Vd = V_beta / Udc1;
//    float Vq = V_alpha / Udc1;
//    // 对称SVPWM参数
//    const float Vmax = 0.5f;
//    const float Vmin = -0.5f;
//    
//    // Clarke逆变换生成三相电压
//    const float sqrt31 = sqrtf(3.0f);
//    float Ua = Vd;
//    float Ub = (-0.5f * Vd) + (sqrt31/2 * Vq);
//    float Uc = (-0.5f * Vd) - (sqrt31/2 * Vq);
//
//    // 改进的扇区判断（TI标准方法）
//    float Vref1 = Vq;
//    float Vref2 = (sqrt31 * Vd - Vq) / 2.0f;
//    float Vref3 = (-sqrt31 * Vd - Vq) / 2.0f;
//    
//    int sector = 0;
//    if (Vref1 > 0) sector |= 0x01;
//    if (Vref2 > 0) sector |= 0x02;
//    if (Vref3 > 0) sector |= 0x04;
//    
//    // 标准扇区映射表
//    const int sector_map[8] = {0, 2, 6, 1, 4, 3, 5, 0};
//    sector = sector_map[sector];
//
//    // 电压偏移（对称调制）
//    float Voffset = (Vmax + Vmin - (Ua + Ub + Uc)) / 3.0f;
//    Ua += Voffset;
//    Ub += Voffset;
//    Uc += Voffset;
//
//    // 计算占空比并限幅
//    Ta = fmaxf(fminf((Ua - Vmin)/(Vmax - Vmin), 1.0f), 0.0f);
//    Tb = fmaxf(fminf((Ub - Vmin)/(Vmax - Vmin), 1.0f), 0.0f);
//    Tc = fmaxf(fminf((Uc - Vmin)/(Vmax - Vmin), 1.0f), 0.0f);
//
//    // 相位调整（按标准扇区重新映射）
//    switch(sector) {
//        case 1:  // Sector I (0-60°)
//            SWAP(Tb, Tc);
//            SWAP(Ta, Tb);
//            break;
//        case 2:  // Sector II (60-120°)
//            SWAP(Ta, Tc);
//            SWAP(Tb, Tc);
//            break;
//        case 3:  // Sector III (120-180°)
//            SWAP(Ta, Tb);
//            SWAP(Ta, Tc);
//            break;
//        case 4:  // Sector IV (180-240°)
//            SWAP(Ta, Tc);
//            SWAP(Tb, Ta);
//            break;
//        case 5:  // Sector V (240-300°)
//            SWAP(Tb, Tc);
//            SWAP(Ta, Tb);
//            break;
//        case 6:  // Sector VI (300-360°)
//            SWAP(Ta, Tb);
//            SWAP(Tc, Tb);
//            break;
//        default: // 错误处理（输出50%占空比）
//            Ta = Tb = Tc = 0.5f;
//    }
//
//    // 设置PWM输出
//    tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_1, (uint16_t)(Ta*PWM_PERIOD));
//    tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_2, (uint16_t)(Tb*PWM_PERIOD));
//    tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_3, (uint16_t)(Tc*PWM_PERIOD));
//}









//void SVPWM_Calculate_GPT_2(float V_alpha, float V_beta) {
//    float Ta, Tb, Tc;
//    
//    float Vd = V_alpha / Udc;
//    float Vq = V_beta / Udc;
//
//    float T1, T2, T0;
//    float sector;
//    float Vref = sqrtf(Vd * Vd + Vq * Vq);
//    float angle = atan2f(Vq, Vd);  // 计算角度时使用归一化后的Vd/Vq
//
//    // 角度范围修正到0~2π
//    if (angle < 0) angle += 2 * PI;
//
//    /*----- 过调制处理核心算法 -----*/
//    const float Vmax_linear = 1.0f / sqrtf(3.0f); // 线性区最大幅值 ≈0.577
//    const float Vmax_overmod = 2.0f / 3.0f;        // 过调制II区幅值 ≈0.666
//
//    // 过调制I区处理（0.577 < Vref ≤ 0.666）
//    if (Vref > Vmax_linear && Vref <= Vmax_overmod) {
//        float angle_mod = fmodf(angle, PI/3);      // 当前扇区内的余角
//        float adjusted_angle = angle_mod - PI/6;   // 转换为-30°~30°
//        float denominator = sqrtf(3.0f) * cosf(adjusted_angle);
//        denominator = fmaxf(denominator, 1e-6f);  // 避免除零
//        Vref = fminf(Vref, 1.0f / denominator);   // 限制幅值到六边形边界
//    }
//    // 过调制II区处理（Vref > 0.666）
//    else if (Vref > Vmax_overmod) {
//        Vref = Vmax_overmod;                      // 幅值锁定为六边形边沿
//        angle = floorf(angle / (PI/3)) * (PI/3) + PI/6; // 角度对齐到六边形边沿
//    }
//
//    /*----- 扇区计算与PWM生成 -----*/
//    sector = floorf(angle / (PI/3)) + 1;          // 计算修正后的扇区
//    float angle_in_sector = fmodf(angle, PI/3);   // 扇区内局部角度
//
//    // 过调制II区特殊处理（六步模式）
//    if (Vref >= Vmax_overmod) {
//        // 六步模式占空比直接分配（每个矢量占50%时间）
//        switch ((int)sector) {
//            case 1: Ta = 1.0f; Tb = 0.5f; Tc = 0.0f; break;
//            case 2: Ta = 0.5f; Tb = 1.0f; Tc = 0.0f; break;
//            case 3: Ta = 0.0f; Tb = 1.0f; Tc = 0.5f; break;
//            case 4: Ta = 0.0f; Tb = 0.5f; Tc = 1.0f; break;
//            case 5: Ta = 0.5f; Tb = 0.0f; Tc = 1.0f; break;
//            case 6: Ta = 1.0f; Tb = 0.0f; Tc = 0.5f; break;
//        }
//    } 
//    // 常规SVPWM计算（包含过调制I区）
//    else {
//        // 计算基本矢量作用时间
//        float t1 = Vref * sinf(PI/3 - angle_in_sector) / sinf(PI/3);
//        float t2 = Vref * sinf(angle_in_sector) / sinf(PI/3);
//        T0 = (1.0f - t1 - t2) / 2.0f;
//
//        // 分配各相占空比
//        switch ((int)sector) {
//            case 1: Ta = t1+t2+T0; Tb = t2+T0;    Tc = T0;    break;
//            case 2: Ta = t1+T0;    Tb = t1+t2+T0; Tc = T0;    break;
//            case 3: Ta = T0;       Tb = t1+t2+T0; Tc = t2+T0; break;
//            case 4: Ta = T0;       Tb = t1+T0;    Tc = t1+t2+T0; break;
//            case 5: Ta = t2+T0;    Tb = T0;       Tc = t1+t2+T0; break;
//            case 6: Ta = t1+t2+T0; Tb = T0;       Tc = t1+T0;    break;
//        }
//    }
//
//    // PWM占空比设置（假设PWM周期已正确定义）
//    tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_1, (uint16_t)(Ta*PWM_PERIOD));
//    tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_2, (uint16_t)(Tb*PWM_PERIOD));
//    tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_3, (uint16_t)(Tc*PWM_PERIOD));
//}













void InverseParkTransform(float V_d, float V_q, float theta, float *V_alpha, float *V_beta) {
    float cos_theta = cosf(theta);
    float sin_theta = sinf(theta);
    *V_alpha = V_d * cos_theta - V_q * sin_theta;
    *V_beta = V_d * sin_theta + V_q * cos_theta;
}

//void Angle_To_Cos_Sin(float angle_temp,TRANSF_COS_SIN_DEF* cos_sin_temp)
//{
//  cos_sin_temp->Cos = arm_cos_f32(angle_temp);
//  cos_sin_temp->Sin = arm_sin_f32(angle_temp);
//}













//获取相线电流,三电阻采样
void current_read_foc_3shunt(void)
{
  static int16_t a_temp,b_temp,c_temp;

//   static int16_t counter;
//          //counter= tmr_counter_value_get(TMR1);
      
//        counter++;
       
//        if(counter==16000)
//        {
//          counter=0;
//          printf("b\n");
//        }
  
  
  c_temp = I_offset.Ic - adc_preempt_conversion_data_get(ADC1, ADC_PREEMPT_CHANNEL_1);
  a_temp = I_offset.Ia - adc_preempt_conversion_data_get(ADC1, ADC_PREEMPT_CHANNEL_2);
  b_temp  =I_offset.Ib - adc_preempt_conversion_data_get(ADC1, ADC_PREEMPT_CHANNEL_3);
  
  
//  c_temp = adc_preempt_conversion_data_get(ADC1, ADC_PREEMPT_CHANNEL_1)-I_offset.Ic;
//  a_temp = adc_preempt_conversion_data_get(ADC1, ADC_PREEMPT_CHANNEL_2)-I_offset.Ia;
//  b_temp = adc_preempt_conversion_data_get(ADC1, ADC_PREEMPT_CHANNEL_3)-I_offset.Ib;
  
//  c_temp = adc_preempt_conversion_data_get(ADC1, ADC_PREEMPT_CHANNEL_1);
//  a_temp = adc_preempt_conversion_data_get(ADC1, ADC_PREEMPT_CHANNEL_2);
//  b_temp =adc_preempt_conversion_data_get(ADC1, ADC_PREEMPT_CHANNEL_3);

  
  
  //三角函数计算所用
//  switch(sector_f)
//	{
//		case 4:
//		case 5:
//			I_abc.Ia = a_temp*SAMPLE_CURR_CON_FACTOR_1;
//			I_abc.Ib = b_temp*SAMPLE_CURR_CON_FACTOR_1;
//                        I_abc.Ic = -I_abc.Ia-I_abc.Ib;
//			break;
//		case 6:
//		case 1:
//            
//            I_abc.Ib = b_temp*SAMPLE_CURR_CON_FACTOR_1;
//            I_abc.Ic = c_temp*SAMPLE_CURR_CON_FACTOR_1;
//            I_abc.Ia = -I_abc.Ib-I_abc.Ic;
//			break;
//		case 2:
//		case 3:
//            I_abc.Ia = a_temp*SAMPLE_CURR_CON_FACTOR_1;
//            I_abc.Ic = b_temp*SAMPLE_CURR_CON_FACTOR_1;
//			I_abc.Ib = -I_abc.Ia-I_abc.Ic ;
//			break;
//		default:
//            I_abc.Ia = a_temp*SAMPLE_CURR_CON_FACTOR_1;
//            I_abc.Ib = b_temp*SAMPLE_CURR_CON_FACTOR_1;
//            I_abc.Ic = b_temp*SAMPLE_CURR_CON_FACTOR_1;
//			break;			
//	}

  
  //直接计算扇区所需进行的采样
switch(sector_f)
	{
		case 4:
		case 6:
			I_abc.Ia = a_temp*SAMPLE_CURR_CON_FACTOR_1;
			I_abc.Ib = b_temp*SAMPLE_CURR_CON_FACTOR_1;
                        I_abc.Ic = -I_abc.Ia-I_abc.Ib;
			break;
		case 3:
		case 2:
            
            I_abc.Ib = b_temp*SAMPLE_CURR_CON_FACTOR_1;
            I_abc.Ic = c_temp*SAMPLE_CURR_CON_FACTOR_1;
            I_abc.Ia = -I_abc.Ib-I_abc.Ic;
			break;
		case 1:
		case 5:
            I_abc.Ia = a_temp*SAMPLE_CURR_CON_FACTOR_1;
            I_abc.Ic = b_temp*SAMPLE_CURR_CON_FACTOR_1;
			I_abc.Ib = -I_abc.Ia-I_abc.Ic ;
			break;
		default:
            I_abc.Ia = a_temp*SAMPLE_CURR_CON_FACTOR_1;
            I_abc.Ib = b_temp*SAMPLE_CURR_CON_FACTOR_1;
            I_abc.Ic = b_temp*SAMPLE_CURR_CON_FACTOR_1;
			break;			
	}

}


//获取电流偏置
void Get_Current_Offset(uint16_t num_samples) {
    int32_t sum_ia = 0, sum_ib = 0, sum_ic = 0;

    

    // 采集多组电流采样值
    for (uint16_t i = 0; i < num_samples; i++) {
     
        
        I_offset.Ia =adc_preempt_conversion_data_get(ADC1, ADC_PREEMPT_CHANNEL_2) ;
        I_offset.Ib =adc_preempt_conversion_data_get(ADC1, ADC_PREEMPT_CHANNEL_3) ;
        I_offset.Ic =adc_preempt_conversion_data_get(ADC1, ADC_PREEMPT_CHANNEL_1) ;


        // 累加采样值
        sum_ia += I_offset.Ia;
        sum_ib += I_offset.Ib;
        sum_ic += I_offset.Ic;

        // 延时，确保采样间隔
       
        wk_delay_ms(1);
    }

    // 计算平均值作为电流偏置
    I_offset.Ia = sum_ia / num_samples;
    I_offset.Ib = sum_ib / num_samples;
    I_offset.Ic = sum_ic / num_samples;
}


float get_encode_angle(void)
{

  return fmodf((float)((TMR2->cval) * (2.0f * pi) / (4*ENCODER_PPR-1 ))*POLE_PAIR_NUM,2*pi);
         
}



void Start_Up(void)
{
    //角度对齐
    static u32 wTimebase=0;
	wTimebase++;

	if(wTimebase<=ANGULAR_ALIGNMENT_TIME)
	{



	Voltage_DQ.Vd=3.0f;
    Voltage_DQ.Vq=0.0f;
    
    theta=0.0f;
    
    InverseParkTransform(Voltage_DQ.Vd,Voltage_DQ.Vq,theta,&Voltage_Alpha_Beta.Ialpha,&Voltage_Alpha_Beta.Ibeta);
    
    SVPWM_Calculate_GPT_2(Voltage_Alpha_Beta.Ialpha,Voltage_Alpha_Beta.Ibeta);


	}
	else
	{
          
        //PID_Init();
		//wTimebase = 0;  

		Voltage_DQ.Vd =Voltage_DQ.Vq=0.0f;
		
		
		
        //清空编码定时器计数器
        tmr_counter_value_set(TMR2, 0);


        //启动电机
        State= STOP;
        //停止电机
	//State=RUN;
                
                
                
                
	}
}


//停止电机，停止输出PWM并置零
void Stop(void)
{
    tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_1,FALSE);
    tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_2,FALSE);
    tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_3,FALSE);

    tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_1C,TRUE);
    tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_2C,TRUE);
    tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_3C,TRUE);
    tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_1, 0);
    tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_2, 0);
    tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_3, 0);


    //电流清零
    I_abc.Ia=0.0f;
    I_abc.Ib=0.0f;
    I_abc.Ic=0.0f;
    I_Alpha_Beta.Ialpha=0.0f;
    I_Alpha_Beta.Ibeta=0.0f;
    I_DQ.Id=0.0f;
    I_DQ.Iq=0.0f;
    //电压清零
    Voltage_DQ.Vd=0.0f;
    Voltage_DQ.Vq=0.0f;
    Voltage_Alpha_Beta.Ialpha=0.0f;
    Voltage_Alpha_Beta.Ibeta=0.0f;
   
    Iq_ref=0.0f;

    pid_seepd.integral=0.0f;
    pid_seepd.prev_error=0.0f;
    pid_seepd.output=0.0f;

    pid_current_Iq.integral=0.0f;
    pid_current_Iq.prev_error=0.0f;
    pid_current_Iq.output=0.0f;

    pid_current_Id.integral=0.0f;
    pid_current_Id.prev_error=0.0f;
    pid_current_Id.output=0.0f;



}






//clark变换
void ClarkeTransform(CURRENT_ABC_DEF Iabc, CURRENT_ALPHA_BETA_DEF* IalphaBeta)
{
    // IalphaBeta->Ialpha = Iabc.Ia;
    // IalphaBeta->Ibeta = (Iabc.Ib - Iabc.Ic) / 2.0f;
    IalphaBeta->Ialpha = (Iabc.Ia - (Iabc.Ib + Iabc.Ic) * 0.5F) * 2.0F / 3.0F;
    IalphaBeta->Ibeta = (Iabc.Ib - Iabc.Ic) * 0.866025388F * 2.0F / 3.0F;
    
}

//park变换
void ParkTransform(CURRENT_ALPHA_BETA_DEF IalphaBeta, float theta, CURRENT_DQ_DEF* Idq)
{
    Idq->Id = IalphaBeta.Ialpha * cosf(theta) + IalphaBeta.Ibeta * sinf(theta);
    Idq->Iq = -IalphaBeta.Ialpha * sinf(theta) + IalphaBeta.Ibeta * cosf(theta);
}


// 初始化PID控制器

void PID_Init(PIDController *pid, float Kp, float Ki, float Kd, float max_output, float min_output, float max_integral, float min_integral) {
    pid->Kp = Kp;
    pid->Ki = Ki;
    pid->Kd = Kd;
    pid->prev_error = 0.0f;
    pid->integral = 0.0f;
    pid->output = 0.0f;
    pid->max_output = max_output;
    pid->min_output = min_output;
    pid->max_integral = max_integral;
    pid->min_integral = min_integral;
}
// 计算PID输出，并实现积分限幅和输出限幅
float PID_Compute(PIDController *pid, float setpoint, float measured_value) {
    // 计算当前误差
    float error = setpoint - measured_value;
    
    // 计算积分项
    pid->integral += error;

    // 积分限幅：确保积分项在最大值和最小值之间
//    if (pid->integral > pid->max_integral) {
//        pid->integral = pid->max_integral;
//    } else if (pid->integral < pid->min_integral) {
//        pid->integral = pid->min_integral;
//    }

    // 计算微分项
    float derivative = error - pid->prev_error;

    // 计算PID输出
    pid->output = pid->Kp * error + pid->Ki * pid->integral + pid->Kd * derivative;

    // 输出限幅：确保输出在最大值和最小值之间
    if (pid->output > pid->max_output) {
        pid->output = pid->max_output;
    } else if (pid->output < pid->min_output) {
        pid->output = pid->min_output;
    }

    // 更新前一误差
    pid->prev_error = error;

    // 返回控制输出
    return pid->output;
}


// void speed_computer(int32_t encode_now, uint8_t ms)
// {
//     uint8_t i = 0, j = 0;
//     float temp = 0.0;
//     static uint8_t sp_count = 0, k = 0;
//     static float speed_arr[10] = {0.0};                     /* 存储速度进行滤波运算 */
//
//     if (sp_count == ms)                                     /* 计算一次速度 */
//     {
//         /* 计算电机转速 
//            第一步 ：计算ms毫秒内计数变化量
//            第二步 ；计算1min内计数变化量：g_encode.speed * ((1000 / ms) * 60 ，
//            第三步 ：除以编码器旋转一圈的计数次数（倍频倍数 * 编码器分辨率）
//            第四步 ：除以减速比即可得出电机转速
//         */
//         g_encode.encode_now = encode_now;                                /* 取出编码器当前计数值 */
//         g_encode.speed = (g_encode.encode_now - g_encode.encode_old);    /* 计算编码器计数值的变化量 */
//        
//         speed_arr[k++] = (float)(g_encode.speed * ((1000 / ms) * 60.0) / REDUCTION_RATIO / (ROTO_RATIO) );    /* 保存电机转速 */
//        
//         g_encode.encode_old = g_encode.encode_now;          /* 保存当前编码器的值 */
//
//         /* 累计10次速度值，后续进行滤波*/
//         if (k == 10)
//         {
//             for (i = 10; i >= 1; i--)                       /* 冒泡排序*/
//             {
//                 for (j = 0; j < (i - 1); j++) 
//                 {
//                     if (speed_arr[j] > speed_arr[j + 1])    /* 数值比较 */
//                     { 
//                         temp = speed_arr[j];                /* 数值换位 */
//                         speed_arr[j] = speed_arr[j + 1];
//                         speed_arr[j + 1] = temp;
//                     }
//                 }
//             }
//            
//             temp = 0.0;
//            
//             for (i = 2; i < 8; i++)                         /* 去除两边高低数据 */
//             {
//                 temp += speed_arr[i];                       /* 将中间数值累加 */
//             }
//            
//             temp = (float)(temp / 6);                       /*求速度平均值*/
//            
//             /* 一阶低通滤波
//              * 公式为：Y(n)= qX(n) + (1-q)Y(n-1)
//              * 其中X(n)为本次采样值；Y(n-1)为上次滤波输出值；Y(n)为本次滤波输出值，q为滤波系数
//              * q值越小则上一次输出对本次输出影响越大，整体曲线越平稳，但是对于速度变化的响应也会越慢
//              */
//             speed_motor = (s16)( ((float)0.48 * temp) + (speed_motor * (float)0.52) );
//             k = 0;
//         }
//         sp_count = 0;
//     }
//     sp_count ++;
// }


void speed_computer(int32_t encode_now, uint8_t ms)
{
    uint8_t i = 0, j = 0;
    float temp = 0.0;
    static uint8_t sp_count = 0, k = 0;
    static float speed_arr[5] = {0.0};                     /* 存储速度进行滤波运算 */

    if (sp_count == ms)                                     /* 计算一次速度 */
    {
        /* 计算电机转速 
           第一步 ：计算ms毫秒内计数变化量
           第二步 ；计算1min内计数变化量：g_encode.speed * ((1000 / ms) * 60 ，
           第三步 ：除以编码器旋转一圈的计数次数（倍频倍数 * 编码器分辨率）
           第四步 ：除以减速比即可得出电机转速
        */
        g_encode.encode_now = encode_now;                                /* 取出编码器当前计数值 */
        g_encode.speed = (g_encode.encode_now - g_encode.encode_old);    /* 计算编码器计数值的变化量 */
        
       speed_motor = (float)(g_encode.speed * ((1000 / ms) * 60.0) / REDUCTION_RATIO / (ROTO_RATIO) );    /* 保存电机转速 */
        
        g_encode.encode_old = g_encode.encode_now;          /* 保存当前编码器的值 */

        /* 累计10次速度值，后续进行滤波*/
//        if (k == 5)
//        {
//            for (i = 5; i >= 1; i--)                       /* 冒泡排序*/
//            {
//                for (j = 0; j < (i - 1); j++) 
//                {
//                    if (speed_arr[j] > speed_arr[j + 1])    /* 数值比较 */
//                    { 
//                        temp = speed_arr[j];                /* 数值换位 */
//                        speed_arr[j] = speed_arr[j + 1];
//                        speed_arr[j + 1] = temp;
//                    }
//                }
//            }
//            
//            temp = 0.0;
//            
//            for (i = 1; i < 4; i++)                         /* 去除两边高低数据 */
//            {
//                temp += speed_arr[i];                       /* 将中间数值累加 */
//            }
//            
//            temp = (float)(temp / 3);                       /*求速度平均值*/
//            
//            /* 一阶低通滤波
//             * 公式为：Y(n)= qX(n) + (1-q)Y(n-1)
//             * 其中X(n)为本次采样值；Y(n-1)为上次滤波输出值；Y(n)为本次滤波输出值，q为滤波系数
//             * q值越小则上一次输出对本次输出影响越大，整体曲线越平稳，但是对于速度变化的响应也会越慢
//             */
//            
//            speed_motor = (s16)( ((float)0.4f * temp) + (speed_motor * (float)0.6f) );
//            k = 0;
//        }
        
        
       // speed_motor = (s16)( ((float)0.1f * temp) + (speed_motor * (float)0.9f) );
        
        sp_count = 0;
    }
    sp_count ++;
}



int32_t gtim_get_encode(void)
{
    return ( int32_t )(TMR2->cval)+g_timx_encode_count*3999 ;   /* 总的编码器值 = 当前计数值 + 之前累计编码器的值 */
  
}



//处理串口接收到的数据
void process_received_data(uint8_t* data, uint8_t length) {
   static uint8_t frame_header;
   frame_header  = atoi(&data[0]);
   static  uint8_t speed_temp[5];
   static  uint8_t iq_P[7];
   static  uint8_t iq_I[7];
   static  uint8_t IQ_ref_set_temp[4];
    switch (frame_header)
    {
    case 1:                                //帧头为1，电机状态
        State= atoi(&data[2]);
        /* code */
        break;
    case 2:                                //帧头为2，电机转速
     speed_temp[0]=data[2];
      speed_temp[1]=data[3];
      speed_temp[2]=data[4];
      speed_temp[3]=data[5];
      speed_temp[4]=data[6];
        speed_set = atoi(speed_temp);
         break;
    case 3:                                //帧头为3，电机电流
    iq_P[0]=data[2];
    iq_P[1]=data[3];
    iq_P[2]=data[4];
    iq_P[3]=data[5];
    iq_P[4]=data[6];
    iq_P[5]=data[7];
    iq_P[6]=data[8];
    pid_iq_p = atof(iq_P);
        break;
    case 4:                                //帧头为4，电机电流
    iq_I[0]=data[2];
    iq_I[1]=data[3];
    iq_I[2]=data[4];
    iq_I[3]=data[5];
    iq_I[4]=data[6];
    iq_I[5]=data[7];
    iq_I[6]=data[8];
    pid_iq_i = atof(iq_I);
        break;
    case 5:                                //帧头为5，电机电流
    IQ_ref_set_temp[0]=data[2];
    IQ_ref_set_temp[1]=data[3];
    IQ_ref_set_temp[2]=data[4];
    IQ_ref_set_temp[3]=data[5];
    IQ_ref_set = atof(IQ_ref_set_temp);
    default:
        break;
    }


    memset(data, 0, DMA1_CHANNEL1_BUFFER_SIZE); // 清空接收缓冲区
    
    
    
}

//状态切换
void state_switch(void)
{
    switch (State)
    {
    case START:
       Start_Up();
        break;
    case RUN:
        
        foc_current_loop();
        break;
    case STOP:
         Stop();    
        break;
    case STOP_TO_RUN:
        stop_to_run();
        break;
        
    default:
}
}


//停止状态跳转运行状态前准备
// 停止到运行状态
// 停止到运行状态
void stop_to_run(void)
    // 启用定时器1的通道1
{
    // 启用定时器1的通道2
    tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_1,TRUE);
    // 启用定时器1的通道3
    tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_2,TRUE);
    tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_3,TRUE);
    // 启用定时器1的通道1C

    // 启用定时器1的通道2C
    tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_1C,TRUE);
    // 启用定时器1的通道3C
    tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_2C,TRUE);
    tmr_channel_enable(TMR1, TMR_SELECT_CHANNEL_3C,TRUE);
    // 设置状态为运行

    State = RUN;
}




void Modulate_Vec_Tim(float* T1, float* T2) //输入的T1,T2是两个非零矢量的工作时间长度，这里要求线性区加和不大于32768
{
  float P1 = *T1, P2 = *T2;
  float PSum = P1 + P2;
  if(PSum > 1.0f)//过调制模块,1区
  {
    P1 =  P1 / PSum;
    P2 =  P2 / PSum;		
    if(PSum > 1.154f)//过调制模块，2区，32768*1.154过调制模块，大过1倍长度区，看P1,P2哪个大把时间往哪边偏移,构造线性连续偏移即可
    {
	float PsumDelta = PSum - 1.154f;			
	if(P1 > P2)
	{
	  if(P2 < PsumDelta)
	  {
	    P2 = 0.0f;		
	    P1 = 1.0f;
	  }	
	  else
	  {
	    P2 -= PsumDelta;		
	    P1 += PsumDelta;
	  }						
        }
        else
        {
	  if(P1 < PsumDelta)
	 {
	   P1 = 0.0f;		
	   P2 = 1.0f;
	 }	
	 else
        {
	  P1 -= PsumDelta;		
	  P2 += PsumDelta;
        }	
      }	
    }
   *T1 = P1;//赋值操作
   *T2 = P2;
  }	
}






